#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray, Float64
import time

class MoveNullspaceCommander(Node):
    def __init__(self):
        super().__init__('move_nullspace_commander')

        self.pub_move_joint = self.create_publisher(Float64MultiArray, '/command_move_joint', 10)
        self.pub_move_nullspace = self.create_publisher(Float64, '/command_move_nullspace', 10)

        self.move_joint_cmd = [-1.215456, 0.4159984588623047, 0.4808502197265625, -0.7959480285644531, 1.7736701965332031, -0.8081560134887695, -2.131875991821289]
        self.move_nullspace_cmds = [-1.5, 3.0, -1.5]

        self.publish_all()

    def wait_for_subscribers(self, timeout=1.0):
        start = time.time()
        while time.time() - start < timeout:
            if self.pub_move_joint.get_subscription_count() > 0 and self.pub_move_nullspace.get_subscription_count() > 0:
                return True
            self.get_logger().info("Waiting for subscribers...")
            time.sleep(0.1)
        self.get_logger().warn("Timeout waiting for subscribers.")
        return False

    def publish_all(self):
        self.wait_for_subscribers(timeout=1.0)

        # 发送 joint 指令
        msg_joint = Float64MultiArray()
        msg_joint.data = self.move_joint_cmd
        self.pub_move_joint.publish(msg_joint)
        self.get_logger().info(f"Published move_joint_cmd: {msg_joint.data}")
        time.sleep(5)  # 等一等，确保控制器收到

        # 顺序发送 nullspace 指令
        for val in self.move_nullspace_cmds:
            msg_nullspace = Float64()
            msg_nullspace.data = val
            self.pub_move_nullspace.publish(msg_nullspace)
            self.get_logger().info(f"Published move_nullspace_cmd: {val}")
            time.sleep(3)  # 每条之间留一点时间

def main():
    rclpy.init()
    node = MoveNullspaceCommander()
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
